

whilecount=0;
heading=0:45:360;
obstaclespd=1;
realboatobstacle=2;
for whichheading=1:length(headings)
    
whilecount=whilecount+1;

startime=clock;

%% find where robot is, and obstacle is. 
% need to include the definition of stat


% robotstat = getStatus(realboatrobot);
% 
% while (robotstat.started == 1)
%     pause(1);
%     robotstat = getStatus(realboatrobot);
% end

obstaclestat = getStatus(realboatobstacle);



%latitude >> robotX=(robotstat.lat-startlat)*111043.07209801719;
%longitude >> robotX=(robotstat.lon-startlon)*-79.947035;
%latitude >> obstacleX=(obstaclestat.lat-startlat)*111043.07209801719;
%longitude >> obstacleX=(obstaclestat.lon-startlon)*111120;

obstacleangle=obstaclestat.compass*2*pi/360;
disp(obstacleangle)
% velocityangle=robotstat.compass*2*pi/360;

%need to get compass (convert from degrees to radians >>velocityangle), latitude/longitude (and from that
%derive cartesian coordinates for program)


    %% go where I wanna go
%     desiredRoblon=startlon+(desiredrobotX)/84843.20730071979;
%     desiredRoblat=endlat+desiredrobotY/111043.07209801719;
%     desiredObstaclelon=startlon+(desiredobstacleX)/84843.20730071979;
%     desiredObstaclelat=endlat+desiredobstacleY/111043.07209801719;
    sendCommand(realboatobstacle, 'goToHeading',heading(whichheading),obstaclespd);
    
%     sendCommand(realboatobstacle, 'goToWaypoint', [desiredObstaclelon,desiredObstaclelat,.5,(obstaclespd/timeinterval)]);
    
    %%
    
    endtime=clock;
    
    pause(5);
    
%     pause(timeinterval-(endtime-startime))
    
%     %deleting all
%      if ishandle(goalscatter)
%          delete(goalscatter);
%      end
%      if COLREG==0
%      else
%      delete(textcolreg)
%      end
%      delete(submarine)
%     delete(robotscatter);
% %     delete(obstaclescatter);
%     %     delete(VOpatch);
%     delete(robot);
%     delete(boat);
end


goalreached=1;

%  text(robotX,robotY,' \leftarrow robot&submarine','FontSize',20)
% text(obstacleX,obstacleY,' \leftarrow ship-in-the-way','FontSize',20)
